#include "AvoidanceBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "SeekBehaviour.h"
#include "BPhysicsMotionState.h"
#include "InteractiveState.h"
#include "WanderBehaviour.h"
#include "FleeBehaviour.h"
#include "PathFollowBehaviour.h"

void AvoidanceBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    //check validity
    if(!info->GetSourceActor() || info->GetActorVector()->size() <= 0)
        return;

    //// BroadPhase/*
    vActors narrow;
    vActors* all =  info->GetActorVector();
    for(int i = 0; i < all->size(); ++i)
    {
        if( (*all)[i]->GetAiCollisionRadius() <= 0 ||
           !(*all)[i]->IsAwake())
            continue;
        float distSquared = (info->GetSourceActor()->GetPosition() -
                             (*all)[i]->GetPosition()).length2();
        float squaredRadii = (info->GetSourceActor()->GetAiCollisionRadius() * 
                              info->GetSourceActor()->GetAiCollisionRadius()) + 
                             ((*all)[i]->GetAiCollisionRadius() * 
                              (*all)[i]->GetAiCollisionRadius());
        if(squaredRadii < distSquared)
            narrow.push_back((*all)[i]);
    }
    //Inverse Matrix
    //make a matrix with the x axis pointing in the direction of the bodies velocity
    btMatrix3x3 m;
    btVector3 x = info->GetSourceActor()->GetHeading();
    x.setY(0);
    if(x.length2() < 0.001f)
        x.setValue(0,0,1);
    x.normalize();
    btVector3 y(0,1,0);
    btVector3 z = y.cross(x);
    z.normalize();
    m[0] = x;//x
    m[1] = y; //y
    m[2] = z; // z  
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(),
         info->GetSourceActor()->GetPosition()+x * 25, btVector3(1,0,0));
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(),
        info->GetSourceActor()->GetPosition()+y * 25, btVector3(0,1,0));
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(),
        info->GetSourceActor()->GetPosition()+z * 25, btVector3(0,0,1));

    btTransform t(m, info->GetSourceActor()->GetPosition());
    btTransform inv = t.inverse();
    //so we dont have to recalculate data while finding the closest distance
    AvoidanceCache cache;
    float extrudeDist = info->GetExtrudeDistance() * info->GetSourceActor()->GetLinearVelocity().length();
    //take all obstacles into local space,
    for(int i = 0; i < narrow.size(); ++i)
    {
        btVector3 posWorld = narrow[i]->GetBodyPointer()->getWorldTransform().getOrigin();
        btVector3 posLocal = inv * posWorld;
        float rad = narrow[i]->GetAiCollisionRadius();
        if(posLocal.x() > 0 && posLocal.x() < extrudeDist) //SCALE THIS BY VELOCITY
        {
            //if the ABS(y) value of obstacles location
            //is LESS than our adius, it is overlapping
            float joinedRadii = info->GetSourceActor()->GetAiCollisionRadius() + rad;
            if(abs(posLocal.z()) < joinedRadii)
            {
                //now to get the x co-ord of the collision
                float pos[2] = {posLocal.x(),posLocal.x()};
                float z = posLocal.z();
                float val = rad*rad - z*z;
                if(val > 0)
                {
                    float res = sqrt(val);
                    pos[0] += res;
                    pos[1] -= res;
                }
                float smallest = min(pos[0], pos[1]);
                if(smallest < cache.smallestDistance)
                {
                    cache.index = i;
                    cache.positionLocal = posLocal;
                    cache.smallestDistance = smallest;
                }
            }                           
        }
    }
    float wanderVolume = 1.0f;
    //ok so we now have the closest obstacle
    if(cache.index != -1 )
    {
        //Store the scombined radii of the two objects
        float radii  = narrow[cache.index]->GetAiCollisionRadius() + 
                       info->GetSourceActor()->GetAiCollisionRadius();
        //The Z component is in a direction away from the sphere
        float zComponent = (radii - cache.positionLocal.z());
        //if it is a positive position, we need the value to be negative
        //this is caused by the inverse matrix multiplication
        if(cache.positionLocal.z() > 0)
            zComponent *= -1;

        //the distance of the nearest point
        float dist = cache.positionLocal.length() - radii;
        if(dist < 0)
            dist *= -1; 
        //multiply the z components direction by this amount
        zComponent *= (radii * info->GetAvoidFactor()) / dist;
        //the x component slows us down in the direction away from the object
        float xComponent = -dist;
        if(info->GetAvoidStopFactor() != 0)
            xComponent *= (radii * info->GetAvoidFactor()) / (dist * info->GetAvoidStopFactor());
        //construt the force vector in local space
        btVector3 contactPosLocal(xComponent,0,zComponent);
        //transform it back into world space
        btVector3 contactPosWorld = t * contactPosLocal;
        contactPosWorld.setY(0);
        //extract the differene in position
        btVector3 force = contactPosWorld - info->GetSourceActor()->GetPosition();
        //scale it by the maximum velocity
        force.normalize();
        force *= info->GetMaxVelocity();
        //bluntly apply the force
        BehaviourI::FillArray(motion, &force, info->GetAvoidanceWeight());
        //draw the force line, and contact pos
        Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(),
          info->GetSourceActor()->GetPosition() + btVector3(motion[0],motion[1],motion[2]), btVector3(1,1,0));
        btVector3 lp = t*cache.positionLocal;
        Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(),lp,btVector3(1,0,0));
        //make sure our other behaviour doesnt over-ride this force
        wanderVolume = 1.0f - info->GetAvoidanceWeight();
    }
    ////Seek to the new0 location 
    if(m_wander == NULL) 
    {
        if(info->GetTargetActor())
            m_wander  = new PathFollowBehaviour();
        else
            m_wander = new WanderBehaviour();
    }
    m_wander->CalcMotion(motion, info, wanderVolume);
}